/*
 * SemolThread.cpp
 *
 *  Created on: 2013-02-01
 *      Author: mathieu
 */

#include "semol/core/SemolThread.h"

#include "semol/core/Semol.h"
#include "semol/core/SemolEvent.h"
#include "semol/core/SensorimotorEvent.h"
#include "semol/core/ParamEvent.h"

#include <utilite/UImageEvent.h>
#include <utilite/UAudioEvent.h>
#include <utilite/UEventsManager.h>
#include <utilite/UEventsHandler.h>

namespace semol {

SemolThread::SemolThread() :
	semol_(new Semol()),
	sensorsBufferMaxSize_(0)
{
}

SemolThread::~SemolThread() {
	UEventsManager::removeHandler(this);

	// Stop the thread first
	this->join(true);

	delete semol_;
}

void SemolThread::pushNewState(State newState, const ParametersMap & parameters)
{
	ULOGGER_DEBUG("to %d", newState);

	stateMutex_.lock();
	{
		state_.push(newState);
		stateParam_.push(parameters);
	}
	stateMutex_.unlock();

	sensorimotorAdded_.release();
}

void SemolThread::clearBufferedSensors()
{
	UScopeMutex sm(sensorimotorMutex_);
	sensorimotorBuffer_.clear();
}

void SemolThread::mainLoopKill()
{
	this->clearBufferedSensors();

	// this will post the newData semaphore
	sensorimotorAdded_.release();
}

void SemolThread::mainLoop()
{
	State state = kStateDetecting;
	ParametersMap parameters;

	stateMutex_.lock();
	{
		if(!state_.empty() && !stateParam_.empty())
		{
			state = state_.top();
			state_.pop();
			parameters = stateParam_.top();
			stateParam_.pop();
		}
	}
	stateMutex_.unlock();

	switch(state)
	{
	case kStateDetecting:
		this->process();
		break;
	case kStateChangingParameters:
		semol_->parseParameters(parameters);
		break;
	case kStateReseting:
		semol_->resetMemory();
		this->clearBufferedSensors();
		break;
	case kStateDumpingMemory:
		semol_->dumpData();
		break;
	case kStateDumpingPrediction:
		semol_->dumpPrediction();
		break;
	case kStateGeneratingGraph:
		semol_->generateGraph(parameters.at("path"));
		break;
	case kStateGeneratingLocalGraph:
		semol_->generateGraph(parameters.at("path"), atoi(parameters.at("id").c_str()), atoi(parameters.at("margin").c_str()));
		break;
	case kStateDeletingMemory:
		semol_->resetMemory(true);
		this->clearBufferedSensors();
		break;
	case kStateCleanSensorsBuffer:
		this->clearBufferedSensors();
		break;
	default:
		UFATAL("Invalid state !?!?");
		break;
	}
}

void SemolThread::handleEvent(UEvent* event)
{
	if(this->isRunning() && event->getClassName().compare("UImageEvent") == 0)
	{
		std::list<SensorRaw> sensors;
		UImageEvent * e = (UImageEvent*)event;
		if(e->getCode() == UImageEvent::kCodeImage)
		{
			sensors.push_back(SensorRaw(Sensor::kTypeImage, e->image()));
		}
		if(sensors.size())
		{
			this->addSensorimotor(sensors, std::list<Actuator>());
		}
	}
	else if(this->isRunning() && event->getClassName().compare("UAudioEvent") == 0)
	{
		std::list<SensorRaw> sensors;
		UAudioEvent * e = (UAudioEvent*)event;
		if(e->getCode() == UAudioEvent::kTypeFrameFreqSqrdMagn)
		{
			sensors.push_back(SensorRaw(Sensor::kTypeAudioFreqSqrdMagn, Sensor::fromAudio(e->frameFreq())));
		}
		if(sensors.size())
		{
			this->addSensorimotor(sensors, std::list<Actuator>());
		}
	}
	else if(this->isRunning() && event->getClassName().compare("SensorimotorEvent") == 0)
	{
		SensorimotorEvent * e = (SensorimotorEvent*)event;
		if(e->getCode() == SensorimotorEvent::kTypeData)
		{
			this->addSensorimotor(e->sensors(), e->actuators());
		}
	}
	else if(event->getClassName().compare("SemolEventCmd") == 0)
	{
		SemolEventCmd * rtabmapEvent = (SemolEventCmd*)event;
		SemolEventCmd::Cmd cmd = rtabmapEvent->getCmd();
		if(cmd == SemolEventCmd::kCmdResetMemory)
		{
			ULOGGER_DEBUG("CMD_RESET_MEMORY");
			pushNewState(kStateReseting);
		}
		else if(cmd == SemolEventCmd::kCmdDumpMemory)
		{
			ULOGGER_DEBUG("CMD_DUMP_MEMORY");
			pushNewState(kStateDumpingMemory);
		}
		else if(cmd == SemolEventCmd::kCmdDumpPrediction)
		{
			ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
			pushNewState(kStateDumpingPrediction);
		}
		else if(cmd == SemolEventCmd::kCmdGenerateGraph)
		{
			if(!rtabmapEvent->getStr().empty())
			{
				ULOGGER_DEBUG("CMD_GENERATE_GRAPH");
				ParametersMap param;
				param.insert(ParametersPair("path", rtabmapEvent->getStr()));
				pushNewState(kStateGeneratingGraph, param);
			}
		}
		else if(cmd == SemolEventCmd::kCmdGenerateLocalGraph)
		{
			std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';');
			if(values.size() == 3)
			{
				ULOGGER_DEBUG("CMD_GENERATE_LOCAL_GRAPH");
				ParametersMap param;
				param.insert(ParametersPair("path", *values.begin()));
				param.insert(ParametersPair("id", *(++values.begin())));
				param.insert(ParametersPair("margin", *values.rbegin()));
				pushNewState(kStateGeneratingLocalGraph, param);
			}
		}
		else if(cmd == SemolEventCmd::kCmdDeleteMemory)
		{
			ULOGGER_DEBUG("CMD_DELETE_MEMORY");
			pushNewState(kStateDeletingMemory);
		}
		else if(cmd == SemolEventCmd::kCmdCleanSensorsBuffer)
		{
			ULOGGER_DEBUG("CMD_CLEAN_SENSORS_BUFFER");
			pushNewState(kStateCleanSensorsBuffer);
		}
	}
	else if(event->getClassName().compare("ParamEvent") == 0)
	{
		ULOGGER_DEBUG("changing parameters");
		pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
	}
}

void SemolThread::process()
{
	std::list<SensorRaw> sensors;
	std::list<Actuator> actuators;
	this->getSensorimotor(sensors, actuators);

	semol_->process(sensors, actuators);
	Statistics stats = semol_->getStatistics();

	stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)sensorimotorBuffer_.size());
	ULOGGER_DEBUG("posting stat event...");
	this->post(new SemolEvent(stats));

}

void SemolThread::addSensorimotor(const std::list<SensorRaw> & sensors, const std::list<Actuator> & actuators)
{
	UDEBUG("sensors %d, actuators %d", sensors.size(), actuators.size());
	if(!sensors.size() && !actuators.size())
	{
		ULOGGER_ERROR("Sensors and actuators empty !?");
		return;
	}

	bool notify = true;
	sensorimotorMutex_.lock();
	{
		sensorimotorBuffer_.push_back(std::make_pair(sensors, actuators));
		while(sensorsBufferMaxSize_ > 0 && sensorimotorBuffer_.size() >= (unsigned int)sensorsBufferMaxSize_)
		{
			ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
			sensorimotorBuffer_.pop_front();
			notify = false;
		}
	}
	sensorimotorMutex_.unlock();

	if(notify)
	{
		sensorimotorAdded_.release();
	}
}

void SemolThread::getSensorimotor(std::list<SensorRaw> & sensors, std::list<Actuator> & actuators)
{
	ULOGGER_DEBUG("");
	sensors.clear();
	actuators.clear();

	ULOGGER_INFO("waiting for data");
	sensorimotorAdded_.acquire();
	ULOGGER_INFO("wake-up");

	sensorimotorMutex_.lock();
	{
		if(!sensorimotorBuffer_.empty())
		{
			sensors = sensorimotorBuffer_.front().first;
			actuators = sensorimotorBuffer_.front().second;
			sensorimotorBuffer_.pop_front();
		}
	}
	sensorimotorMutex_.unlock();
}

} //end namespace semol
